% save('data/experiment-60min.mat', 'inst_count_vec', 'rt_vec');
% load('data/experiment-60min.mat');
classdef test_lqr_kalman_less < handle 
    %UNTITLED Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        % model=[];
        inst_count_vec=[];
        rt_vec=[]; 
         m=[];
         m_inst_count_vec=[];
         m_rt_vec=[]; 
         samplingTime = 1;         
         lqr=[];
         x0=[];
    end
    
    methods

%  clear all
%  clear
%  load('data/experiment-60min.mat');
%  open policy/test_fake_lqr.m
%  test_fake_lqr().build_model( inst_count_vec, rt_vec)
 function obj=test_lqr_kalman_less()
            
        end
        
       function [m_x,d_x]=linearize(obj,x)
            m_x = mean(x);
            % d_x = x-m_x;
            d_x = x-ones(size(x,1),1)*m_x;
        end
        
        function d_x=linearize_around(obj,x,m_x)
            d_x = x-ones( size(x,1),1) * m_x ;
        end
        
        function x=delinearize_around(obj,d_x,m_x)
            x = ones( size(d_x,1),1) * m_x  + d_x;
        end
        
      
        function build_model(obj, inst_count_vec, rt_vec)
            obj.inst_count_vec=inst_count_vec;  
            obj.rt_vec=rt_vec;
            %----------
            n_st=1;% 1 state variable
            n_in= size(inst_count_vec,2);  %11 inputs
            n_out= 1; %size(rt_vec,2); % 1 output
            n_env=1; % environmental disturbances
            A=ones(n_st,n_st);
            B=ones(n_st,n_in);
            K=ones(n_st,n_env);
            C=eye(n_out,n_st);
            D=zeros(n_out,n_in);
            
            
            [m_rt_vec,              d_rt_vec]= obj.linearize(rt_vec);
            [m_inst_count_vec,  d_inst_count_vec] = obj.linearize(inst_count_vec); 
            
            
%             m_rt_vec=0; %2
%             [  d_rt_vec]= obj.linearize_around(rt_vec , m_rt_vec);  %  2  seconds desired
%              m_inst_count_vec= ones(1, size(inst_count_vec,2))* 0; %2
%           [ d_inst_count_vec] = obj.linearize_around(inst_count_vec,...
%                         m_inst_count_vec);
            
            u =  d_inst_count_vec;
            y = d_rt_vec;
            
            data=iddata(y, u , obj.samplingTime);
            % m=idss(A,B,C,D,K,'Ts', obj.samplingTime,'DisturbanceModel','Estimate','InitialState','Estimate');
            m=idss(A,B,C,D,K,'Ts', obj.samplingTime,'DisturbanceModel','None','InitialState','Estimate');
%             m0.As = [NaN,0;0,NaN];
%             m0.Bs = [NaN;NaN];

            m.Cs = C;
            m1=init(m);
            mm=pem(data,m1);

            obj.m=mm;
            obj.m_inst_count_vec=m_inst_count_vec; 
            obj.m_rt_vec = m_rt_vec; 
            
            %-------------------------------------------------
        %    slower= tf(20,[1 20],1);
            slower=ss(0,0,0,1,1);
       %       slower.OutputName='slower_out';
          integrator=ss(1,1,1,0,1);
   %       integrator=ss(0,0,0,1,1);

            sys = ss(mm,'m'); 
       %       sys.OutputName='sys_out';
            % Design LQ-optimal gain K            
%         K = lqry(sys,10,1)  % u = -Kx minimizes J(u)
            % Add integrator state dz/dt = -phi
            A_aug = [1 0;
                            0 sys.a];
%            B_aug = [zeros(1,11) ; sys.b];
            B_aug = [-1*ones(1,11) %-1 0 -1 1 1 -1 1 1 -1 -1 1               %-1*ones(1,11) 
                            sys.b];

            % LQR gain synthesis
            Q = blkdiag(1, 0.1);            % for integrated difference and for crappy state (here response time) 
            R = diag(prices_amz().get_hourly_price);
            N=zeros(1,11);
            [K,S,e]  = dlqr(A_aug,B_aug,Q,R);            
            
            %F.InputName = 'r';
    %        sum_er = sumblk('sum_out','slower_out','sys_out','+-');            
            
               [Kest,L,P]  = kalman(sys , [], 0.0001,0);         
              Kest = ss([0 0;0 Kest.a],[zeros(1,12) 1; Kest.b 0],[0 1;1 0;0 1],zeros(3,13),1);
      %       Kest = ss([0 0;0 Kest.a],[zeros(1,12) 1; Kest.b 0],[0 1;1 0;0 1],zeros(3,13),1);
    %        reference_plus_
      %        gain = ss(0,[0 0],zeros(11,1),-1*K,1);                 
             gain = lqgreg(Kest,K);       
          
            
   
            blk_sys=append(slower,  integrator, gain, sys); 
            Q1 = [2  1  -14
                    4  2    0
                    3  14  0];
             Q2= [(5:15)' (3:13)' zeros(11,1)];
%           Q2=[];
%             Q_to_kalman = [(16:26)' (3:13)' zeros(11,1)
%                                        27 14 0
                                       
             Q=[Q1 ; Q2];                    
            % sys_all = connect(blk_sys,Q,[1 (5:15)],14);  input  = [zeros(11,60)      ]; 
            sys_all = connect(blk_sys,Q,[1],[14 (3:13)]);  input  = [ ]; 
            reference = [ones(1,20)*8 ones(1,20)*6  ones(1,20)*7];    
            d_reference = obj.linearize_around(reference , m_rt_vec);  %  2  seconds desired
            

   %         input  = [zeros(10,60) ; [ones(1,20)*4 ones(1,20) zeros(1,20)]       ]; 
            t= 0:1:60-1;
%             reference = [4 4 4];      
 %            input  = zeros(11,3); 
%             t= 0:1:3-1;
            % impulseplot(slower);
           % step(slower) 

%            sys_out=ones(1,60);
%             lsim(sys, input ,t);

         [y,t,x] =    lsim(sys_all, [d_reference ; input] ,t); 
         figure
         u=UtilityLib();              
         [0.4,0.4,0.4]
         % plot(reference, strcat(u.lnstyle{2}),...
         plot(d_reference, strcat(u.lnstyle{2}),...             
                'LineWidth',2,...                             
                'MarkerSize',10);    
            hold on;
        % plot(obj.delinearize_around(y(:,1) , m_rt_vec), strcat(u.lnstyle{3},u.color{7}),...
         plot(y(:,1) , strcat(u.lnstyle{3},u.color{7}),...
                'LineWidth',2,...                             
                'MarkerSize',10);    
         legend('Reference','System')
         title('System/Reference response time');   
         
         figure;
         alloc_instances =  obj.delinearize_around(y(:,2:12) , m_inst_count_vec);  
         % plot_instances(alloc_instances); legend(prices_amz().get_api_names');
         plot_instances(y(:,2:12)); legend(prices_amz().get_api_names');
         legend(prices_amz().get_api_names')
         title('Number of Instances');   
         
      %      lsim(series(slower,sys2),integrator, [reference] ,t); 

%      lsim(integrator,reference,t);
            
             
             
%      %       clsys = feedback(sys,F,[1 2 3 4 5 6 7 8 9 10 11],[1],+1);
%              distrb=([ones(1,20) zeros(1,40)])'*10+randn(60,1);
%              u=[zeros(60,11) distrb]; 
%               t= 0:1:60-1;
%              %[y_c,t_c,x_c] =
%              lsim(clsys,u,t); 
            
%             N=zeros(1,11); 
%             % P = sys_noise(:,[1 1]);
%             % [K,S,e] = lqr(mm.A,mm.B,Q,R,N)  doesnt consider noise
%             % [K,S,e] = lqr(sys,Q,R,N) 
%             % [K,S,e] = lqrd(sys.a,sys.b,Q,R,1);  -----
%              [K,S,e] =  lqr(sys,Q,R,N)
%             % Design Kalman state estimator Kest.
%             % noise covariance data of the plant  
%             Qk=eye(11);
%             Rk=eye(1);
%             Nk=zeros(11,1);
%  %            [Kest,L,P] = kalman(sys,Qk,Rk,Nk,[1 1 1 1 1 1 1 1 1 1 1], [1])
%             [Kest,L,P]  = kalman(sys_noise, 0, 100,0)
%             % Kest = kalman(sys_noise,1,0.01)
%            % [Kest,L,P] = kalman(sys_noise,1,0.01); 
%             
%             % Form LQG regulator = LQ gain + Kalman filter.
%             F = lqgreg(Kest,K);
%             obj.lqr=F;
%             
% %             clsys = feedback(sys_noise,F,+1);
%             clsys = feedback(sys_noise,F,[1 2 3 4 5 6 7 8 9 10 11],[1],+1);
%             
            %   distrb=randn(60,1)*0.1;
           %  distrb=sin((1:60)'*0.1)*4+randn(60,1)*0.1;
%             figure;   
            % [y_o,t_o,x_o] =
      %       lsim(sys_noise,u,t);
%             ltiview(clsys,'r-.',sys_noise,'m--');
% %              ltiview();
% %              ltiview(sys_noise);
%             u=UtilityLib(); 
%              figure;
%              plot( y_c,  u.color{1}); hold on; 
%              plot( y_c,  u.color{2}); 
%             rt_acc = obj.delinearize_around(rt_acc,m_rt_vec); 
%             plot(rt_acc); 

            %---------------------------------- way 1
%             noise=randn(60,2)*0.001; 
%             x_sys = mm.X0; % system state  for the first time
%             rt_acc=[];
%             u_opt_acc=[];
%             x_ctrl=0;
%             for i=1:60                
%                 rt_ = rt_vec(end,:);  % system output - kalman input for the first time                
%                 u_opt=F.c*x_ctrl + F.d * rt_;
%                 x_ctrl=F.a* x_ctrl + F.b * rt_;
%                 %------ system update ------
%                 rt_=sys_noise.c*x_sys + sys_noise.d*[u_opt ; noise(i,1)];
%                 x_sys =sys_noise.a*x_sys + sys_noise.b*[u_opt ; noise(i,1)];
%                 rt_acc=[rt_acc rt_];
%                 u_opt_acc=[u_opt_acc ; u_opt'];
%             end
%             rt_acc = obj.delinearize_around(rt_acc,m_rt_vec); 
%             plot(rt_acc); 
% 
%             %---------------------------------- way 2
%             %      inp=rand
%             noise=randn(60,2);
%             
%             x_kalm = mm.X0 % kalman state for the first time
%             x_sys = mm.X0; % system state  for the first time
%             x_lqr=0;
%             
%             rt_ = rt_vec(end,:);  % system output - kalman input for the first time
%             u_opt = inst_count_vec(end,:)';  % controller output-kalman input for the first time
%             
%             
%             rt_acc=[];            
%             u_opt_acc=[]; 
%             for i=1:60
%                 %------  kalman update ------
%                 tmp=Kest.c*x_kalm + Kest.d*[u_opt ; rt_]; 
%                 y1_e=tmp(1);
%                 x1_e = tmp(2); 
%                 x_kalm =Kest.a*x_kalm + Kest.b*[u_opt ; rt_]; 
%                                 
%                 %------ controller update ------
%                  u_opt =-1*K*x1_e;
%                  x_lqr=(Kest.a-L*Kest.c-(Kest.B-L*Kest.D)*K)*x1_e+L*rt_;
%                 % u_opt = [1 0 0 0 0 0 0 0 0 0 0]'; 
%                 
%                 %------ system update ------
%                 rt_=sys_noise.c*x_sys + sys_noise.d*[u_opt ; noise(i,1)];
%                 x_sys =sys_noise.a*x_sys + sys_noise.b*[u_opt ; noise(i,1)]; 
%                 rt_acc=[rt_acc rt_]; 
%                 u_opt_acc=[u_opt_acc ; u_opt'];
%             end
%         %    clsys = feedback(sys,F,+1);
            
        
        end        
        
    end
    
end

            